#line 1 "D:/Archivio/mikroC/tric-ricotta/motors.c"
#line 1 "d:/archivio/mikroc/tric-ricotta/config.h"
#line 1 "d:/archivio/mikroc/tric-ricotta/motors.h"



void MOTORS_update(unsigned short *MOTORS_speed);
void MOTORS_esc_startup();
void MOTORS_esc_config();
#line 11 "D:/Archivio/mikroC/tric-ricotta/motors.c"
void MOTORS_update(unsigned short *MOTORS_speed) {
 unsigned short cnt;
 unsigned short MOTORS_biggest_speed;

 MOTORS_biggest_speed = 0;

 for(cnt=0; cnt<4; cnt++) {
 if(MOTORS_speed[cnt] >  250 ) MOTORS_speed[cnt] =  250 ;
 MOTORS_biggest_speed = (MOTORS_speed[cnt] >= MOTORS_biggest_speed) ? MOTORS_speed[cnt] : MOTORS_biggest_speed;
 }

 for(cnt=0; cnt<=MOTORS_biggest_speed; cnt++) {

 if(cnt == MOTORS_speed[0])  PORTC.B0  = 1;
 if(cnt == MOTORS_speed[1])  PORTC.B1  = 1;
 if(cnt == MOTORS_speed[2])  PORTC.B6  = 1;
 if(cnt == MOTORS_speed[3])  PORTC.B7  = 1;

 Delay_us(2);
 }
}
#line 36 "D:/Archivio/mikroC/tric-ricotta/motors.c"
void MOTORS_esc_startup() {
 unsigned short cnt=0;
 while(cnt<100) {
  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
  PORTC.B7  = 0;
 Delay_ms(1);
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
  PORTC.B7  = 1;
 Delay_ms(20);
 cnt++;
 }
}
#line 60 "D:/Archivio/mikroC/tric-ricotta/motors.c"
void MOTORS_esc_config() {
 unsigned short MOTORS_speed_align[4];
 unsigned short cnt=0;
 MOTORS_speed_align[0] =  250 ;
 MOTORS_speed_align[1] =  250 ;
 MOTORS_speed_align[2] =  250 ;
 MOTORS_speed_align[3] =  250 ;
 while(cnt<100) {
  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
  PORTC.B7  = 0;
 Delay_ms(1);
 MOTORS_update(&MOTORS_speed_align);
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
  PORTC.B7  = 1;
 Delay_ms(20);
 cnt++;
 }
 cnt = 0;
 while(cnt<100) {
  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
  PORTC.B7  = 0;
 Delay_ms(1);
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
  PORTC.B7  = 1;
 Delay_ms(20);
 cnt++;
 }
 Delay_ms(2000);
}
